
import socket
import json
import copy
import time

import yaml
import os
import traceback

import rospy
from geometry_msgs.msg import Point
from acupoint_msgs.msg import AcupointCoordinate,LegCoordinate
from std_msgs.msg import Bool
from std_msgs.msg import Int64

class JakaCommunicate:
    def __init__(self):
        self.actions=self.loadYaml('./test.yaml')
        if self.actions is None:
            print("Error, load yaml file failed")
            return
        self.actions=self.actions['actions']
        
        self.flag=-1
        self.acupoint_name=None
        self.acupoint_coordinate=None
        self.left_coordinate=[0,0,0]
        self.right_coordinate=[0,0,0]
        self.acupoint_sub = rospy.Subscriber("/acupoint/acu2base_coordinate", AcupointCoordinate, self.acupointCallback) 
        self.left_leg_sub = rospy.Subscriber("/acupoint/left_leg_line", LegCoordinate, self.leftLegCallback) 
        self.right_leg_sub = rospy.Subscriber("/acupoint/right_leg_line", LegCoordinate, self.rightLegCallback) 
        self.jaka_arrive_pub=rospy.Publisher("/jaka_arrive",Bool,queue_size=1)

        while self.acupoint_coordinate is None or self.left_coordinate[0]==0 or self.right_coordinate[0]==0:
            rospy.sleep(0.1)
        print("action start")
       
        x=-1
        y=-1
        z=-1
        try:
            self.client.connect(('10.161.229.76',56789))
            try:
     
                
                for action in self.actions:
                    method=action['method']
                    if method==3:
                        if action['name']=='left_leg':
                            x=self.left_coordinate[0]
                            y=self.left_coordinate[1]
                            z=self.left_coordinate[2]
                        elif action['name']=='right_leg':
                            x=self.right_coordinate[0]
                            y=self.right_coordinate[1]
                            z=self.right_coordinate[2]
                        else:
                            print("undefined name in tweak")
                    else:
                        index=self.acupoint_name.index(action['name'])
                        x=self.acupoint_coordinate[index].x
                        y=self.acupoint_coordinate[index].y
                        z=self.acupoint_coordinate[index].z
                    # y=-178.40056950919865
                    # x=-730.8301589958692
                    # z=172.45003406724504
                    # y=-142.39448286325387
                    # x= -703.9163577106395
                    # z=151.79152922013668
                   
                    data={'method':action['method'],'time':action['time'],'x':x,'y':y,'z':z}
                    data_json=json.dumps(data)
                    print(data_json)
                    wait_key = raw_input(">")
                    if wait_key=='y':
                        # self.client.send(data_json.encode())
                        # recv_data=self.client.recv(1024)
                        # if not recv_data:
                        #     break
                        jaka_arrived_msg=Bool()
                        jaka_arrived_msg=True
                        self.jaka_arrive_pub.publish(jaka_arrived_msg)
                    else:
                        pass
            except:
                print("error during actions")
                print(traceback.format_exc())
        except:
            print("connect failed")
            print(traceback.format_exc())
        finally:
            self.client.close()
        
    def acupointCallback(self,msg):
        self.acupoint_name=copy.deepcopy(msg).name
        self.acupoint_coordinate=copy.deepcopy(msg).acupoints
        # print("u are in callback")
    
    
    def leftLegCallback(self,msg):
        self.left_coordinate[0]=(msg.position[0].x+msg.position[1].x)/2
        self.left_coordinate[1]=(msg.position[0].y+msg.position[1].y)/2
        self.left_coordinate[2]=(msg.position[0].z+msg.position[1].z)/2
    
    def rightLegCallback(self,msg):
        self.right_coordinate[0]=(msg.position[0].x+msg.position[1].x)/2
        self.right_coordinate[1]=(msg.position[0].y+msg.position[1].y)/2
        self.right_coordinate[2]=(msg.position[0].z+msg.position[1].z)/2
        
    def loadYaml(self,file_path):
        if not os.path.exists(file_path):
            print("Error, %s not exists"%file_path)
            return
        yaml_data=None
        try:
            with open(file_path) as f:
                yaml_data=yaml.safe_load(f.read())
        except:
            print("Some error occured,the exception message is:")
            print(traceback.format_exc())
        return yaml_data


if __name__ == "__main__":
    rospy.init_node('nuc_communicate')
    jc=JakaCommunicate()



    

    
